#
# Copyright (C) 2023, Inria
# GRAPHDECO research group, https://team.inria.fr/graphdeco
# All rights reserved.
#
# This software is free for non-commercial, research and evaluation use 
# under the terms of the LICENSE.md file.
#
# For inquiries contact  george.drettakis@inria.fr
#

import torch
from functools import reduce
import numpy as np
from torch_scatter import scatter_max
from utils.general_utils import inverse_sigmoid, get_expon_lr_func
from torch import nn
import os
import faiss
from einops import repeat
from utils.system_utils import mkdir_p
from plyfile import PlyData, PlyElement
from simple_knn._C import distCUDA2
from utils.graphics_utils import BasicPointCloud
from utils.general_utils import strip_symmetric, build_scaling_rotation
from scene.embedding import Embedding, MLP, PosEmbedding
from arguments import ModelParams, OptimizationParams

    
class GaussianModel:

    def setup_functions(self):
        def build_covariance_from_scaling_rotation(scaling, scaling_modifier, rotation):
            L = build_scaling_rotation(scaling_modifier * scaling, rotation)
            actual_covariance = L @ L.transpose(1, 2)
            symm = strip_symmetric(actual_covariance)
            return symm
        
        self.scaling_activation = torch.exp
        self.scaling_inverse_activation = torch.log
        self.covariance_activation = build_covariance_from_scaling_rotation
        self.opacity_activation = torch.sigmoid
        self.inverse_opacity_activation = inverse_sigmoid
        self.rotation_activation = torch.nn.functional.normalize


    def __init__(self, dataset: ModelParams):

        self.feat_dim = dataset.feat_dim
        self.hid_dim = dataset.hid_dim
        self.n_offsets = dataset.n_offsets
        self.voxel_size = dataset.voxel_size

        self.ratio = dataset.ratio
        self.sampling_method = dataset.sampling_method
        self.levels = dataset.levels
                
        self.optimizer = None
        self.percent_dense = 0
        self.spatial_lr_scale = 0
        self.setup_functions()
        
        self.xyz_freqs = 4
        self.dir_freqs = 4
        self.xyz_embedder = PosEmbedding(N_freqs=self.dir_freqs).cuda()
        self.dir_embedder = PosEmbedding(N_freqs=self.dir_freqs).cuda()
                
        for level in range(self.levels):
            opacity_in_dim = self.feat_dim + 3 * (2 * self.dir_freqs + 1)
            cov_in_dim = self.feat_dim + 3 * (2 * self.dir_freqs + 1)
            color_in_dim = self.feat_dim + 3 * (2 * self.dir_freqs + 1)
            offset_in_dim = self.feat_dim
            
            mlp_opacity = MLP(opacity_in_dim, self.hid_dim, self.n_offsets*1, n_layers=2, out_act=nn.Tanh()).cuda()
            mlp_cov = MLP(cov_in_dim, self.hid_dim, self.n_offsets*7, n_layers=2, out_act=None).cuda()
            mlp_color = MLP(color_in_dim, self.hid_dim, self.n_offsets*3, n_layers=2, out_act=nn.Sigmoid()).cuda()
            mlp_offset = MLP(offset_in_dim, self.hid_dim, self.n_offsets*3, n_layers=2, out_act=nn.Tanh()).cuda()
            
            setattr(self, f"mlp_opacity_{level}", mlp_opacity)
            setattr(self, f"mlp_cov_{level}", mlp_cov)
            setattr(self, f"mlp_color_{level}", mlp_color)
            setattr(self, f"mlp_offset_{level}", mlp_offset)
        
    def eval(self):
        for level in range(self.levels):
            getattr(self, f"mlp_opacity_{level}").eval()
            getattr(self, f"mlp_cov_{level}").eval()
            getattr(self, f"mlp_color_{level}").eval()
            getattr(self, f"mlp_offset_{level}").eval()

    def train(self):
        for level in range(self.levels):
            getattr(self, f"mlp_opacity_{level}").train()
            getattr(self, f"mlp_cov_{level}").train()
            getattr(self, f"mlp_color_{level}").train()
            getattr(self, f"mlp_offset_{level}").train()

    def capture(self):
        return (
            self._anchor,
            self._anchor_feat,
            self.optimizer.state_dict(),
            self.spatial_lr_scale)
    
    def restore(self, model_args, training_args):
        (self._anchor,
        self._anchor_feat,
        opt_dict, 
        self.spatial_lr_scale) = model_args
        self.training_setup(training_args)
        self.optimizer.load_state_dict(opt_dict)

    def get_scaling(self, level=0):
        scaling = getattr(self, f"_scaling_{level}")
        return 1.0*self.scaling_activation(scaling)
        
    def get_anchor(self, level=0):
        anchor = getattr(self, f"_anchor_{level}")
        return anchor
    
    def get_anchor_feat(self, level=0):
        anchor_feat = getattr(self, f"_anchor_feat_{level}")
        return anchor_feat
    
    def get_opacity_mlp(self, level=0):
        mlp_opacity = getattr(self, f"mlp_opacity_{level}")
        return mlp_opacity
        
    def get_cov_mlp(self, level=0):
        mlp_cov = getattr(self, f"mlp_cov_{level}")
        return mlp_cov
        
    def get_color_mlp(self, level=0):
        mlp_color = getattr(self, f"mlp_color_{level}")
        return mlp_color
        
    @property
    def get_offset_mlp(self, level=0):
        mlp_offset = getattr(self, f"mlp_offset_{level}")
        return mlp_offset                
            
    def voxelize_sample(self, data=None, voxel_size=0.01):
        np.random.shuffle(data)
        data = np.unique(np.round(data/voxel_size), axis=0)*voxel_size
        return data


    def create_from_pcd(self, pcd: BasicPointCloud, spatial_lr_scale: float):
        self.spatial_lr_scale = spatial_lr_scale
        points = pcd.points
        
        if self.ratio > 0:
            if self.sampling_method == 'uniform':
                points = points[::self.ratio]
            elif self.sampling_method == 'cluster':   
                points = np.ascontiguousarray(points)
                N = int(points.shape[0] // self.ratio)
                kmeans = faiss.Kmeans(d=points.shape[1], k=N, niter=30, verbose=True, gpu=True)
                kmeans.train(points)
                points = kmeans.centroids
            else:
                raise ValueError(f"{self.sampling_method} is not supported!")
        
        if self.voxel_size <= 0:
            init_points = torch.tensor(points).float().cuda()
            init_dist = distCUDA2(init_points).float().cuda()
            median_dist, _ = torch.kthvalue(init_dist, int(init_dist.shape[0]*0.5))
            self.voxel_size = median_dist.item()
            del init_dist
            del init_points
            torch.cuda.empty_cache()

        print(f'Initial voxel_size: {self.voxel_size}')
        points = self.voxelize_sample(points, voxel_size=self.voxel_size)
        
        fused_point_cloud = torch.tensor(np.asarray(points)).float().cuda()  
        anchors_feat = torch.zeros((fused_point_cloud.shape[0], self.feat_dim)).float().cuda()
        
        print("Number of points at initialisation : ", fused_point_cloud.shape[0])

        dist2 = torch.clamp_min(distCUDA2(fused_point_cloud).float().cuda(), 0.0000001)
        scales = torch.log(torch.sqrt(dist2))[..., None]
        
        setattr(self, "_anchor_0", nn.Parameter(fused_point_cloud.requires_grad_(True)))
        setattr(self, "_anchor_feat_0", nn.Parameter(anchors_feat.requires_grad_(True)))
        setattr(self, "_scaling_0", nn.Parameter(scales.requires_grad_(True)))


    def training_setup(self, training_args: OptimizationParams):
        self.percent_dense = training_args.percent_dense
        self.opacity_accum = torch.zeros((self.get_anchor(0).shape[0]*self.n_offsets, 1), device="cuda")
        self.xyz_accum = torch.zeros((self.get_anchor(0).shape[0]*self.n_offsets, 3), device="cuda")
        self.grad_accum = torch.zeros((self.get_anchor(0).shape[0]*self.n_offsets, 1), device="cuda")
        self.demon = torch.zeros((self.get_anchor(0).shape[0]*self.n_offsets, 1), device="cuda")
        
        l = []
        for level in range(self.levels):
            l_level = [
                {'params': getattr(self, f"mlp_opacity_{level}").parameters(), 'lr': training_args.mlp_opacity_lr_init, "name": f"mlp_opacity_{level}"},
                {'params': getattr(self, f"mlp_cov_{level}").parameters(), 'lr': training_args.mlp_cov_lr_init, "name": f"mlp_cov_{level}"},
                {'params': getattr(self, f"mlp_color_{level}").parameters(), 'lr': training_args.mlp_color_lr_init, "name": f"mlp_color_{level}"},
                {'params': getattr(self, f"mlp_offset_{level}").parameters(), 'lr': training_args.mlp_offset_lr_init, "name": f"mlp_offset_{level}"},
            ]
            if level == 0:
                l_level.append({'params': [getattr(self, f"_anchor_feat_{level}")], 'lr': training_args.feature_lr, "name": f"anchor_feat_{level}"})
            l += l_level
            
        self.optimizer = torch.optim.Adam(l, lr=0.0, eps=1e-15)
                
        self.mlp_opacity_scheduler_args = get_expon_lr_func(lr_init=training_args.mlp_opacity_lr_init,
                                                    lr_final=training_args.mlp_opacity_lr_final,
                                                    lr_delay_mult=training_args.mlp_opacity_lr_delay_mult,
                                                    max_steps=training_args.mlp_opacity_lr_max_steps)
        
        self.mlp_offset_scheduler_args = get_expon_lr_func(lr_init=training_args.mlp_offset_lr_init,
                                                    lr_final=training_args.mlp_offset_lr_final,
                                                    lr_delay_mult=training_args.mlp_offset_lr_delay_mult,
                                                    max_steps=training_args.mlp_offset_lr_max_steps)
        
        self.mlp_cov_scheduler_args = get_expon_lr_func(lr_init=training_args.mlp_cov_lr_init,
                                                    lr_final=training_args.mlp_cov_lr_final,
                                                    lr_delay_mult=training_args.mlp_cov_lr_delay_mult,
                                                    max_steps=training_args.mlp_cov_lr_max_steps)
        
        self.mlp_color_scheduler_args = get_expon_lr_func(lr_init=training_args.mlp_color_lr_init,
                                                    lr_final=training_args.mlp_color_lr_final,
                                                    lr_delay_mult=training_args.mlp_color_lr_delay_mult,
                                                    max_steps=training_args.mlp_color_lr_max_steps)

    def update_learning_rate(self, iteration):
        ''' Learning rate scheduling per step '''
        for param_group in self.optimizer.param_groups:
            if "mlp_opacity" in param_group["name"]:
                lr = self.mlp_opacity_scheduler_args(iteration)
                param_group['lr'] = lr
            if "mlp_cov" in param_group["name"]:
                lr = self.mlp_cov_scheduler_args(iteration)
                param_group['lr'] = lr
            if "mlp_color" in param_group["name"]:
                lr = self.mlp_color_scheduler_args(iteration)
                param_group['lr'] = lr
            if "mlp_offset" in param_group["name"]:
                lr = self.mlp_offset_scheduler_args(iteration)
                param_group['lr'] = lr            
            
    def construct_list_of_attributes(self):
        l = ['x', 'y', 'z']
        anchor_feat = getattr(self, "_anchor_feat_0")
        for i in range(anchor_feat.shape[1]):
            l.append('anchor_feat_{}'.format(i))
        return l
     
    def save_npz(self, path):
        mkdir_p(os.path.dirname(path))
        save_dict = dict()
        anchor = getattr(self, "_anchor_0")
        anchor_feat = getattr(self, "_anchor_feat_0")
        save_dict["anchor"] = anchor.detach().cpu().numpy().astype(np.float16)
        save_dict["feat"] = anchor_feat.detach().cpu().numpy().astype(np.float16)
        np.savez(path, **save_dict)


    def load_npz_sparse_gaussian(self, path):
        load_dict = np.load(path, allow_pickle=True)
        print("Loading ", path)
        self._anchor = nn.Parameter(torch.from_numpy(load_dict["anchor"]).cuda().float().requires_grad_(True))
        self._anchor_feat = nn.Parameter(torch.from_numpy(load_dict["feat"]).cuda().float().requires_grad_(True))
        dist2 = torch.clamp_min(distCUDA2(self.get_anchor).float().cuda(), 0.0000001)
        self._scaling = torch.log(torch.sqrt(dist2))[..., None]
        del dist2

    def save_ply(self, path):
        mkdir_p(os.path.dirname(path))
        
        anchor = getattr(self, "_anchor_0").detach().cpu().numpy()
        anchor_feat = getattr(self, "_anchor_feat_0").detach().cpu().numpy()

        dtype_full = [(attribute, 'f4') for attribute in self.construct_list_of_attributes()]
        elements = np.empty(anchor.shape[0], dtype=dtype_full)
        attributes = np.concatenate((anchor, anchor_feat), axis=1)
        elements[:] = list(map(tuple, attributes))
        el = PlyElement.describe(elements, 'vertex')
        PlyData([el]).write(path)

    def load_ply_sparse_gaussian(self, path):
        plydata = PlyData.read(path)

        anchor = np.stack((np.asarray(plydata.elements[0]["x"]),
                        np.asarray(plydata.elements[0]["y"]),
                        np.asarray(plydata.elements[0]["z"])),  axis=1).astype(np.float32)
        anchor = torch.tensor(anchor, dtype=torch.float, device="cuda")
        
        # anchor_feat
        anchor_feat_names = [p.name for p in plydata.elements[0].properties if p.name.startswith("anchor_feat")]
        anchor_feat_names = sorted(anchor_feat_names, key = lambda x: int(x.split('_')[-1]))
        anchor_feats = np.zeros((anchor.shape[0], len(anchor_feat_names)))
        for idx, attr_name in enumerate(anchor_feat_names):
            anchor_feats[:, idx] = np.asarray(plydata.elements[0][attr_name]).astype(np.float32)
        anchor_feats = torch.tensor(anchor_feats, dtype=torch.float, device="cuda")
        
        dist2 = torch.clamp_min(distCUDA2(anchor).float().cuda(), 0.0000001)
        
        setattr(self, "_anchor_0", nn.Parameter(anchor.requires_grad_(True)))
        setattr(self, "_anchor_feat_0", nn.Parameter(anchor_feats.requires_grad_(True)))
        setattr(self, "_scaling_0", torch.log(torch.sqrt(dist2))[..., None])
        del dist2, anchor, anchor_feats


    def replace_tensor_to_optimizer(self, tensor, name):
        optimizable_tensors = {}
        for group in self.optimizer.param_groups:
            if "mlp" in group['name']:
                continue
            if group["name"] == name:
                stored_state = self.optimizer.state.get(group['params'][0], None)
                stored_state["exp_avg"] = torch.zeros_like(tensor)
                stored_state["exp_avg_sq"] = torch.zeros_like(tensor)
                
                del self.optimizer.state[group['params'][0]]
                group["params"][0] = nn.Parameter(tensor.requires_grad_(True))
                self.optimizer.state[group['params'][0]] = stored_state
                optimizable_tensors[group["name"]] = group["params"][0]
        return optimizable_tensors

    def cat_tensors_to_optimizer(self, tensors_dict):
        optimizable_tensors = {}
        for group in self.optimizer.param_groups:
            if "mlp" in group['name']:
                continue
            assert len(group["params"]) == 1
            extension_tensor = tensors_dict[group["name"]]
            stored_state = self.optimizer.state.get(group['params'][0], None)
            if stored_state is not None:
                stored_state["exp_avg"] = torch.cat((stored_state["exp_avg"], torch.zeros_like(extension_tensor)), dim=0)
                stored_state["exp_avg_sq"] = torch.cat((stored_state["exp_avg_sq"], torch.zeros_like(extension_tensor)), dim=0)

                del self.optimizer.state[group['params'][0]]
                group["params"][0] = nn.Parameter(torch.cat((group["params"][0], extension_tensor), dim=0).requires_grad_(True))
                self.optimizer.state[group['params'][0]] = stored_state

                optimizable_tensors[group["name"]] = group["params"][0]
            else:
                group["params"][0] = nn.Parameter(torch.cat((group["params"][0], extension_tensor), dim=0).requires_grad_(True))
                optimizable_tensors[group["name"]] = group["params"][0]
        return optimizable_tensors
    
    
    def _prune_anchor_optimizer(self, mask):
        optimizable_tensors = {}
        for group in self.optimizer.param_groups:
            if  'mlp' in group['name']:
                continue
            stored_state = self.optimizer.state.get(group['params'][0], None)
            if stored_state is not None:
                stored_state["exp_avg"] = stored_state["exp_avg"][mask]
                stored_state["exp_avg_sq"] = stored_state["exp_avg_sq"][mask]

                del self.optimizer.state[group['params'][0]]
                group["params"][0] = nn.Parameter((group["params"][0][mask].requires_grad_(True)))
                self.optimizer.state[group['params'][0]] = stored_state
                optimizable_tensors[group["name"]] = group["params"][0]
            else:
                group["params"][0] = nn.Parameter(group["params"][0][mask].requires_grad_(True))
                optimizable_tensors[group["name"]] = group["params"][0]
        return optimizable_tensors


    def training_statis(self, viewspace_point_tensor, opacity, neural_xyz, visibility_filter, selection_mask, visible_mask):
        tem_anchor_mask = repeat(visible_mask, 'n -> (n k)', k=self.n_offsets)

        tem_selection_mask = torch.zeros_like(selection_mask).bool()
        tem_selection_mask[selection_mask] = visibility_filter
        selection_mask = torch.logical_and(tem_selection_mask, selection_mask)
        
        tem_mask = torch.zeros_like(tem_anchor_mask).bool()
        tem_mask[tem_anchor_mask] = selection_mask
        mask = torch.logical_and(tem_anchor_mask, tem_mask)
        
        self.opacity_accum[mask] += opacity[visibility_filter]
        self.xyz_accum[mask] += neural_xyz[visibility_filter]
        self.grad_accum[mask] += torch.norm(viewspace_point_tensor.grad[visibility_filter,:2], dim=-1, keepdim=True)
        self.demon[mask] += 1       
        del tem_anchor_mask, selection_mask, tem_mask, mask
        
        
    def adjust_anchor(self, min_opacity=0.005, grad_threshold=0.0002):
        mean_opacity = self.opacity_accum / self.demon      # (N*k, 1)
        mean_opacity[mean_opacity.isnan()] = 0.0
        anchor_mean_opacity = mean_opacity.reshape(-1, self.n_offsets).mean(-1) # (N,)
        
        prune_mask = (anchor_mean_opacity < min_opacity)
        self.prune_anchor(prune_mask)
        
        mean_xyz = self.xyz_accum / self.demon   # (N*k, 3)
        mean_xyz[mean_xyz.isnan()] = 0.0
        mean_grad = self.grad_accum / self.demon
        mean_grad[mean_grad.isnan()] = 0.0
        anchor_mean_xyz = mean_xyz.reshape(-1, self.n_offsets, 3)[~prune_mask]
        anchor_mean_grad = mean_grad.reshape(-1, self.n_offsets)[~prune_mask]
        mean_xyz = anchor_mean_xyz.reshape(-1, 3)
        mean_grad = anchor_mean_grad.reshape(-1)
                
        growing_mask = (mean_grad > grad_threshold)
        self.anchor_growing(mean_xyz, growing_mask)
        del mean_opacity, anchor_mean_opacity, prune_mask, mean_xyz, anchor_mean_xyz, growing_mask
        
        self.opacity_accum = torch.zeros((self.get_anchor.shape[0]*self.n_offsets, 1), device="cuda")
        self.xyz_accum = torch.zeros((self.get_anchor.shape[0]*self.n_offsets, 3), device="cuda")
        self.grad_accum = torch.zeros((self.get_anchor.shape[0]*self.n_offsets, 1), device="cuda")
        self.demon = torch.zeros((self.get_anchor.shape[0]*self.n_offsets, 1), device="cuda")
        torch.cuda.empty_cache()


    def prune_anchor(self, prune_mask):
        valid_points_mask = ~prune_mask
        optimizable_tensors = self._prune_anchor_optimizer(valid_points_mask)
        self._anchor = optimizable_tensors["anchor"]
        self._anchor_feat = optimizable_tensors["anchor_feat"]
        dist2 = torch.clamp_min(distCUDA2(self.get_anchor), 0.0000001)
        self._scaling = torch.log(torch.sqrt(dist2))[...,None]
        del dist2

          
    def anchor_growing(self, mean_xyz, growing_mask):
        grid_coords = torch.round(self.get_anchor / self.voxel_size).int()              # origial anchor coords
        candidate_coords = torch.round(mean_xyz[growing_mask] / self.voxel_size).int()  # candidate anchor coords
        candidate_coords_unique, inverse_indices = torch.unique(candidate_coords, return_inverse=True, dim=0)
        
        # add new and remove duplicated anchors
        use_chunk = True
        if use_chunk:
            chunk_size = 4096
            max_iters = grid_coords.shape[0] // chunk_size + (1 if grid_coords.shape[0] % chunk_size != 0 else 0)
            remove_duplicates_list = []
            for i in range(max_iters):
                remove_duplicates = (candidate_coords_unique.unsqueeze(1) == grid_coords[i*chunk_size:(i+1)*chunk_size, :]).all(-1).any(-1).view(-1)
                remove_duplicates_list.append(remove_duplicates)
            remove_duplicates = reduce(torch.logical_or, remove_duplicates_list)
        else:
            remove_duplicates = (candidate_coords_unique.unsqueeze(1) == grid_coords).all(-1).any(-1).view(-1)
        
        remove_duplicates = ~remove_duplicates
        candidate_anchor = candidate_coords_unique[remove_duplicates] * self.voxel_size
        
        if candidate_anchor.shape[0] > 0:
            new_feat = self._anchor_feat.unsqueeze(dim=1).repeat([1, self.n_offsets, 1]).view([-1, self.feat_dim])[growing_mask]
            new_feat = scatter_max(new_feat, inverse_indices.unsqueeze(1).expand(-1, new_feat.size(1)), dim=0)[0][remove_duplicates]
        
            d = {"anchor": candidate_anchor, "anchor_feat": new_feat}
            
            optimizable_tensors = self.cat_tensors_to_optimizer(d)
            self._anchor = optimizable_tensors["anchor"]
            self._anchor_feat = optimizable_tensors["anchor_feat"]
            dist2 = torch.clamp_min(distCUDA2(self.get_anchor), 0.0000001)
            self._scaling = torch.log(torch.sqrt(dist2))[...,None]
            del dist2        

    def save_mlp_checkpoints(self, path):
        mkdir_p(os.path.dirname(path))
        ckpt = {}
        for level in range(self.levels):
            ckpt_level = {
                f'opacity_mlp_{level}': getattr(self, f"mlp_opacity_{level}".state_dict()),
                f'cov_mlp_{level}': getattr(self, f"mlp_cov_{level}".state_dict()),
                f'color_mlp_{level}': getattr(self, f"mlp_color_{level}".state_dict()),
                f'offset_mlp_{level}': getattr(self, f"mlp_offset_{level}".state_dict()),
            }
            ckpt.update(ckpt_level)
        torch.save(ckpt, os.path.join(path, 'checkpoints.pth'))


    def load_mlp_checkpoints(self, path):
        checkpoint = torch.load(os.path.join(path, 'checkpoints.pth'))
        for level in range(self.levels):
            getattr(self, f"mlp_opacity_{level}").load_state_dict(checkpoint[f'opacity_mlp_{level}'])
            getattr(self, f"mlp_cov_{level}").load_state_dict(checkpoint[f'cov_mlp_{level}'])
            getattr(self, f"mlp_color_{level}").load_state_dict(checkpoint[f'color_mlp_{level}'])
            getattr(self, f"mlp_offset_{level}").load_state_dict(checkpoint[f'offset_mlp_{level}'])
